#include <iostream>
#include "Common.h"
using namespace common;

AABB& common::CalcBoundingBox(const std::vector<Eigen::Vector3f>& points) {
    AABB BoundingBox;

    if (points.size() == 0)
    {
        std::cout << "Invalid input!" << std::endl;
        return BoundingBox;
    }
    
    for (int axis = 0; axis < 3; axis++) {
        BoundingBox.min_[axis] = points[0][axis], BoundingBox.max_[axis] = points[0][axis];
        for (int i = 1; i < points.size(); i++) {
            BoundingBox.max_[axis] = std::max(BoundingBox.max_[axis], points[i][axis]);
            BoundingBox.min_[axis] = std::min(BoundingBox.min_[axis], points[i][axis]);
        }
    }
    return BoundingBox;
}

bool common::isBoundingBoxOverlap(const std::vector<Eigen::Vector3f>& points_a, const std::vector<Eigen::Vector3f>& points_b) {
    AABB AABB_a = CalcBoundingBox(points_a);
    AABB AABB_b = CalcBoundingBox(points_b);
    if (AABB_a.max_[0] < AABB_b.min_[0] || AABB_b.max_[0] < AABB_a.min_[0] ||
        AABB_a.max_[1] < AABB_b.min_[1] || AABB_b.max_[1] < AABB_a.min_[1] ||
        AABB_a.max_[2] < AABB_b.min_[2] || AABB_b.max_[2] < AABB_a.min_[2]) {
        return false;
    }
    else return true;
}